﻿using Serialization.Script;
using Microsoft.Xna.Framework;
using System.Diagnostics;
using System;
using System.Collections.Generic;
using PolyLib;
namespace DrawableObjects.Actors.Aircrafts.Controller
{
    /// <summary>
    /// A controller for an aicraft. Orders the aircraft to follow a given path via bezier-curves and shoot their weapons at the players position
    /// </summary>
    public class AIControlledAircraft_PathGuided : AircraftController
    {

        private Path flightPath
        { 
            get;
            set;
        }

        private bool isFinished;

        private Vector2 lastPoint;
        private Vector2 destinationPoint0; // next destination point 
        private Vector2 destinationPoint1; // point after the next destination point
        private Vector2 lastDirection = new Vector2(0,0);
        private const float smoothingPhase = 1000;
        private float smoothingFactor = 0f;

        public override void Update()
        {
            if (!isFinished) {
                Vector2 currentPosition = aircraft.getGamePosition();
                float t = getTFromPosition(lastPoint, destinationPoint0, destinationPoint1, currentPosition);


                // if current Destination is reached, set next destination
                if (t > 0.9f)
                {
                    lastDirection = squareBezierIncline(currentPosition, destinationPoint0, destinationPoint1, t);
                    lastDirection.Normalize();
                    lastDirection *= aircraft.speed;
                    smoothingFactor = smoothingPhase;

                    isFinished = flightPath.isFinished();
                    lastPoint = aircraft.getGamePosition();
                    destinationPoint0 = destinationPoint1;
                    destinationPoint1 = flightPath.getNextDestination();
                }
                if (flightPath.isFinished())
                {
                    aircraft.moveInDirection(Vector2.Subtract( destinationPoint1, aircraft.getGamePosition()));
                }
                else
                {
                    Vector2 moveDirection = squareBezierIncline(currentPosition, destinationPoint0, destinationPoint1, t);
                    moveDirection.Normalize();
                    moveDirection *= aircraft.speed;
                    float smoothingComponent = (float)Math.Pow((double)smoothingFactor / (double)smoothingPhase, 2d);
                    moveDirection = (1 - smoothingComponent) * moveDirection + smoothingComponent * lastDirection;
                    aircraft.moveInDirection(moveDirection);
                }

                if (smoothingFactor >= 0f)
                {
                    float elapsed = GlobalGameClasses.GlobalGameValues.Values.gameTime.ElapsedGameTime.Milliseconds;
                    smoothingFactor = smoothingFactor - elapsed >= 0f ? smoothingFactor - elapsed : 0f;
                }
            }

            fireWeapons();
        }

        /// <summary>
        /// Targets a player aircraft an fires weapons
        /// </summary>
        private void fireWeapons()
        {
            Vector2 gamePosition = aircraft.getGamePosition();
            Actor player = aircraft.world.GetClosestElement(gamePosition, typeof(Aircraft), Owner.PLAYER);
            if (player != null) aircraft.Fire(Vector2.Subtract(player.getGamePosition(), gamePosition));
        }

        /// <summary>
        /// Calculates the derivate of a square bezier function at the given value t. t must be between 0 and 1
        /// </summary>
        private Vector2 squareBezierIncline(Vector2 point0, Vector2 point1, Vector2 point2, float t)
        {
            Debug.Assert(t >= 0 && t <= 1, "t not within [0,1]" );
            Vector2 currentIncline = Vector2.Add(Vector2.Multiply(point0, -2f), Vector2.Multiply(point1, 2f));
            Vector2 temp = Vector2.Add(Vector2.Add(point0, point2), Vector2.Multiply(point1, -2f));
            temp = Vector2.Multiply(temp, (2 * t));
            currentIncline = Vector2.Add(temp, currentIncline);
            return currentIncline;
        }


        private float getTFromPosition(Vector2 point0, Vector2 point1, Vector2 point2, Vector2 currentPoint)
        {
            float a = point2.X;
            float b = point1.X;
            float c = point0.X;
            float d = point2.Y;
            float e = point1.Y;
            float f = point0.Y;
            float x = currentPoint.X;
            float y = currentPoint.Y;

            double A = (Math.Pow(a, 2) + 2 * a * (c - 2 * b) + 4 * Math.Pow(b, 2) - 4 * b * c + Math.Pow(c, 2) + Math.Pow(d, 2) + 2 * d * (f - 2 * e) + Math.Pow(2 * e - f, 2));
            double B = -3 * (Math.Pow(a, 2) + a * (c - 3 * b) + 2 * Math.Pow(b, 2) - b * c + Math.Pow(d, 2) + d * (f - 3 * e) + e * (2 * e - f));
            double C = -(x * (a - 2 * b + c) + y * (d - 2 * e + f) - 3 * Math.Pow(a, 2) + a * (6 * b - c) - 2 * Math.Pow(b, 2) - 3 * Math.Pow(d, 2) + d * (6 * e - f) - 2 * Math.Pow(e, 2));
            double D = x * (a - b) + y * (d - e) - Math.Pow(a, 2) + a * b - d * (d - e);

            List<float> realRoots = getCubicEquationRoots(A, B, C, D);
            A *= 3;
            B *= 2;
            float temp = 0;
            if (realRoots.Count > 0)
            {
                foreach (float root in realRoots)
                {
                    if (Math.Pow(root, 2) * A + root * B + C >= 0.00000000000000000000000000001d)
                    {
                        temp = 1 - root;
                    }
                }
            }
            else
            {
                temp = Vector2.Distance(currentPoint, point0) > Vector2.Distance(currentPoint, point2) ? 1 : 0;
            }
            return temp;
        }

        private List<float> getCubicEquationRoots(double A, double B, double C, double D)
        {
            Polynomial p = new Polynomial(D, C, B, A);
            Complex[] roots = p.Roots();
            List<float> realRoots = new List<float>();
            foreach (Complex root in roots)
            {
                if (root.IsReal() && root.Re >= 0 && root.Re <= 1) realRoots.Add((float)root.Re);
            }

            return realRoots;
        }

        public AIControlledAircraft_PathGuided(Path flightPath)
        {
            this.isFinished = false;
            this.flightPath = flightPath;
            destinationPoint0 = flightPath.getNextDestination();
            destinationPoint1 = flightPath.getNextDestination(); 
        }

        public override void setAircraft(Aircraft aircraft)
        {
            base.setAircraft(aircraft);
            lastPoint = aircraft.getGamePosition();
        }
    }
}

